#include "motor.h"
#include "pstwo.h"
#include "RectTOPolar.h"
#include "led.h"
#include "usart.h"
int LeftHandle_Angle,LeftHandle_Distance;
u16 Default_Counter_Value=65536/2;
u8 Flag_SpeedAdjust=0,Flag_LALMotorAdjust=0,Flag_Fire=0;
float e,e1,e2;
float duk,uk,uk1,out;
float Kp=0.4,Ki=0.2,Kd=0.00;
u16 SubMotor_PWMMax,SubMotor_PWMMin=1;
u16 RevolverMotor_TargetSpeed=600;
long RevolverMotor_RelativeCounter,RevolverMotor_CurrentSpeed;
/*************************
 * 这是使用C8T6对机车所有电机的控制函数,底盘电机是通过修改传给rct6的数据实现的
英文缩写：
MainMotor       底盘主电机//1motor B6line check!
SubMotor        发射结构从电机
RevolverMotor   子弹转盘电机
LALMotor        LockAndLoad（子弹上膛半月板）电机
AcceMotor       加速（摩擦轮）电机
*************************/

void SubMotor_Init(u16 arr,u16 psc)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_OCInitTypeDef  TIM_OCInitStructure;
    TIM_ICInitTypeDef TIM_ICInitStructure;

	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2|RCC_APB1Periph_TIM3, ENABLE);
 	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOC, ENABLE);  //使能GPIO外设时钟使能



    /***********以下是对LALMotor两条输出线的初始化：************/
    GPIO_InitStructure.GPIO_Pin=GPIO_Pin_14|GPIO_Pin_15;
    GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP;
    GPIO_Init(GPIOC,&GPIO_InitStructure);
    GPIO_SetBits(GPIOC,GPIO_Pin_14|GPIO_Pin_15);//L298N两个输出线置高，防止半月板误动



    /***********以下是对TIM3 PWM 输出（端口从A67到B01为轮盘电机，空，摩擦轮1，摩擦轮2）的初始化：************/
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //TIM_CH1
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;  //复用推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1; //TIM_CH1
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;  //复用推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);

	TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值  18000
	TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值  不分频
	TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:80 72000000/（18000*80）=50HZ
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
	TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
    SubMotor_PWMMax=arr-9;


	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; //选择定时器模式:TIM脉冲宽度调制模式2
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
	TIM_OCInitStructure.TIM_Pulse = 0; //设置待装入捕获比较寄存器的脉冲值
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性高
	TIM_OC1Init(TIM3, &TIM_OCInitStructure);  //根据TIM_OCInitStruct中指定的参数初始化外设TIMx
	TIM_OC2Init(TIM3, &TIM_OCInitStructure);
    TIM_OC3Init(TIM3, &TIM_OCInitStructure);
    TIM_OC4Init(TIM3, &TIM_OCInitStructure);

  	//TIM_CtrlPWMOutputs(TIM3,ENABLE);	//MOE 主输出使能
	//TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);  //CH1预装载使能	 
	TIM_ARRPreloadConfig(TIM3, ENABLE); //使能TIMx在ARR上的预装载寄存器
    TIM3->CR2&=0X0000;


    /***********以下是对TIM2正交编码器（轮盘电机测速）输入的初始化：************/
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;    //端口配置
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
    GPIO_Init(GPIOA, &GPIO_InitStructure);                          //根据设定参数初始化GPIOA

    TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
    TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 预分频器
    TIM_TimeBaseStructure.TIM_Period = Default_Counter_Value; //设定计数器自动重装值
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频：不分频
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;////TIM向上计数
    TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
    TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3
    TIM_ICStructInit(&TIM_ICInitStructure);
    TIM_ICInitStructure.TIM_ICFilter = 6;
    TIM_ICInit(TIM2, &TIM_ICInitStructure);
    TIM_ClearFlag(TIM2, TIM_FLAG_Update);//清除TIM的更新标志位
    TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
    
    TIM_Cmd(TIM2, ENABLE);
    TIM_Cmd(TIM3, ENABLE);
	
}
void TIM2_IRQHandler(void) //TIM3中断
{ 
    if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET) //检查 TIM8更新中断发生与否
    { 
        TIM_ClearITPendingBit(TIM2, TIM_IT_Update ); //清除 TIM8更新中断标志
    } 
}
void SubMotor_Control(void)
{
    u8 Flag_RevolverMotorReverse=0;
    
    if(Flag_SpeedAdjust)//轮盘电机常开，保证随时有子弹可以使用
	{
		RevolverMotor_RelativeCounter = (short)TIM2 -> CNT;  
		TIM2 -> CNT=(Default_Counter_Value/2);
        RevolverMotor_CurrentSpeed=RevolverMotor_RelativeCounter-(Default_Counter_Value/2);
		if(RevolverMotor_CurrentSpeed<0)
        {
            RevolverMotor_CurrentSpeed*=-1;
            Flag_RevolverMotorReverse=1;
        }
        else
        {
            Flag_RevolverMotorReverse=0;
        }
		//printf("current motor speed:%ld \r\n",RevolverMotor_CurrentSpeed);
        TIM_SetCompare1(TIM3,PID_Speed_Calc());
        Flag_SpeedAdjust=0;//标志位复位
	}
    if(Flag_Fire)//如果开火启动
    {
        LALMotor_Run();//半月板上膛
    }
    else
    {
        LALMotor_Stop();//半月板停转
    }
    AcceMotor_StatusUpdate();//开火与否写在摩擦轮控制函数里面了
}

//将摇杆的直角坐标转换为极坐标，极角决定前进方向，半径决定前进速度
u8 MainMotor_Control(void)//高四位是方向信息，低四位是速度信息
{
    int PS2_DefaultPosition[2]={128,127};
    u8 Motor_Speed,Motor_Direction;
    RECT_V LeftHandle_Rect;
    POLAR_V LeftHandle_Polar;
    LeftHandle_Rect.x=(double)(PS2_AnologData(PSS_LX)-PS2_DefaultPosition[0]);
    LeftHandle_Rect.y=-((double)(PS2_AnologData(PSS_LY)-PS2_DefaultPosition[1]));
    LeftHandle_Polar=rect_to_polar(LeftHandle_Rect);

    if(LeftHandle_Polar.angle<=22.5&&LeftHandle_Polar.angle>=-22.5)//0000,分别为左、右，10正转，01复转，00刹车
    {
        Motor_Direction=0x09;//1001,left normal,right reverse
    }
    else if(LeftHandle_Polar.angle<=67.5&&LeftHandle_Polar.angle>22.5)
    {
        Motor_Direction=0x08;//1000,left normal,right brake
    }
    else if(LeftHandle_Polar.angle<=112.5&&LeftHandle_Polar.angle>67.5)
    {
        Motor_Direction=0x0A;//1010,left normal,right normal
    }
    else if(LeftHandle_Polar.angle<=157.5&&LeftHandle_Polar.angle>112.5)
    {
        Motor_Direction=0x02;//0010,left brake,right normal
    }
    else if(LeftHandle_Polar.angle>157.5||LeftHandle_Polar.angle<-157.5)//C语言自带标准函数计算所得的极坐标角度介于-180-180之间
    {
        Motor_Direction=0x06;//0110,left reverse,right normal
    }
    else if(LeftHandle_Polar.angle>=-67.5&&LeftHandle_Polar.angle<-22.5)
    {
        Motor_Direction=0x04;//0100,left reverse,right brake
    }
    else if(LeftHandle_Polar.angle>=-112.5&&LeftHandle_Polar.angle<-67.5)
    {
        Motor_Direction=0x05;//0101,left reverse,right reverse
    }
    else if(LeftHandle_Polar.angle>=-157.5&&LeftHandle_Polar.angle<-112.5)
    {
        Motor_Direction=0x01;//0001,left brake,right reverse
    }

    if(LeftHandle_Polar.magnitude<5)
    {
        Motor_Direction=0;//刹车不动。
        Motor_Speed=0;
    }
    else if(LeftHandle_Polar.magnitude<=64)
    {
        Motor_Speed=1;
    }
    else if(LeftHandle_Polar.magnitude)
    {
        Motor_Speed=2;
    }

    return (Motor_Direction<<4)+Motor_Speed;//高四位是方向信息，低四位是速度信息
}

u16 PID_Speed_Calc(void)
{
    u16 Actual_Output;
    e=RevolverMotor_TargetSpeed-RevolverMotor_CurrentSpeed;//计算当前偏差值变量

	duk=(Kp*(e-e1)+Ki*e+Kd*(e-2*e1+e2));//PID连续系统离散化增量型PID算法，算出总偏差值变量。
	uk=duk+uk1;//计算偏差值总变量加上之后偏差值总变量之和赋给目前总偏差值变量
	out=uk;//目前总偏差值变量赋给PID调节后输出偏差值变量
	Actual_Output=out;
	if(Actual_Output>SubMotor_PWMMax)//判断PID调节后输出偏差值变量是否大于额定PWM限制
	{
		Actual_Output=SubMotor_PWMMax;//PID调节后输出偏差值变量为限制内
	}
	else if(Actual_Output<SubMotor_PWMMin)//判断PID调节后输出偏差值变量是否小于0
	{
		Actual_Output=SubMotor_PWMMin;//PID调节后输出偏差值变量为0
	}
	uk1=uk;//目前总偏差值变量赋给之后偏差值总变量
	e2=e1;//之前偏差值变量赋给之后偏差值变量
	e1=e;//当前偏差值变量赋给之前偏差值变量
	return Actual_Output;//PID调节后输出偏差值变量赋给脉冲宽度时间变量
}

void LALMotor_Run(void)
{
    u8 LALMotor_Status[4]={0x03,0x01,0x03,0x02};
    static int LALMotor_StatusPointer=0;
    if(Flag_LALMotorAdjust)
    {
        LED=~LED;
        LALMotor_StatusUpdate(LALMotor_Status[LALMotor_StatusPointer]);
        LALMotor_StatusPointer++;
        if(LALMotor_StatusPointer>3)
        {
            LALMotor_StatusPointer=0;
        }
        Flag_LALMotorAdjust=0;//标志位复位
    }
}
void LALMotor_Stop(void)
{
    LALMotor_StatusUpdate(0x03);
}
void LALMotor_StatusUpdate(u8 Status)//0011刹车，0010正转，0001反转
{
    if(Status&0x02)
        LALMotor_IN1=1;
    else
        LALMotor_IN1=0;

    if(Status&0x01)
        LALMotor_IN2=1;
    else
        LALMotor_IN2=0;
}
void AcceMotor_StatusUpdate(void)
{
    if(Flag_Fire)
    {
        TIM_SetCompare3(TIM3,SubMotor_PWMMax/2);
        TIM_SetCompare4(TIM3,SubMotor_PWMMax/2);
    }
    else
    {
        TIM_SetCompare3(TIM3,SubMotor_PWMMax/10);
        TIM_SetCompare4(TIM3,SubMotor_PWMMax/10);
    }
}
